#pragma once

#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <cstring>
#include <arpa/inet.h>
#include <unistd.h>
#include <mutex>
#include <functional>
#include "mavlink/common/mavlink.h"

class TCPServer {
public:
    using MavlinkMessageCallback = std::function<void(const mavlink_message_t&)>;

    TCPServer(const std::string& ip, int port);
    ~TCPServer();
    void set_mavlink_message_callback(MavlinkMessageCallback callback);

    bool start();

    void send_mavlink_message(const mavlink_message_t& message);

private:
    std::string ip_;
    int port_;
    int server_fd_;
    std::vector<int> clients_;
    std::mutex clients_mutex_;

    void acceptClients();
    void handleClient(int client_fd);
    void handleMavLinkMessage(const std::string& message);

    MavlinkMessageCallback message_callback_;
};